#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
//#include<realsense_dev/depth_value.h>       //自定义一个消息类型

#define PI 3.1415926
#define x_offset  XX //深度相机基座坐标系X
#define y_offset  YY //深度相机基座坐标系Y
#define z_offset  ZZ //深度相机基座坐标系Z

typedef union USHORT_UNION {
    ushort num;
    char c[2];
} ushort_union;

ushort Little(char c0, char c1) {
    ushort_union tmp;
    tmp.c[0] = c0;
    tmp.c[1] = c1;
    return tmp.num;
}


using namespace std;
using namespace cv;

cv::Mat depth_pic;        //定义全局变量，图像矩阵Ｍat形式
std_msgs::Float32MultiArray tgt;
float d_value;
ushort high_value;
float low_value = 6000;//最大深度截止
int tgt_x=0, tgt_y=0; //目标图像坐标
float tgtx = 0, tgty = 0;//目标空间基座系坐标
int cnt = 0;

void depthCallback(const sensor_msgs::Image::ConstPtr&msg)
{
    low_value = 6000;
    int x = 640, y = 480;//深度图像分表率
    int index = y * msg->width + x;
    ushort tmp = Little(msg->data[index*2], msg->data[index*2+1]);//单位mm
    // 也可以转换成深度图
    cv::Mat dep_im = cv::Mat::zeros(msg->height, msg->width, CV_8UC3);
    for (int i = 0; i < msg->height; ++i) {
        for (int j = 0; j < msg->width; j++) {
            int index = i * msg->width + j;
            tmp = Little(msg->data[index * 2], msg->data[index * 2 + 1]);
            if (tmp < low_value && tmp >0)
            {
                low_value = tmp;
                tgt_y = -i + msg->height/2;
                tgt_x = j - msg->width/2;
            }
        }
    }

    if (low_value < 6000 && low_value >0)
    {
        if (cnt < 30)
        {
            tgtx = ( tgtx + float(tgt_x))/2;
            tgty = ( tgty + float(tgt_y))/2;
            cnt = 0;
        }
        cout<<"X: "<<tgt_x<<", Y: "<<tgt_y<<", depth: "<<low_value<<endl;
        cout<<"pX: "<<float(tgt_x)/float(msg->width)*2.0*(low_value*tan(30.0/180.0*PI))<<", pY: "<<float(tgt_y)/float(msg->height)*2.0*(low_value*tan(22.5/180.0*PI))<<", pz: "<<low_value<<endl;
    }
    cnt++;
}



int main(int argc, char **argv)
{
    ros::init(argc, argv, "grasp_depth");               // ros节点初始化
    ros::NodeHandle nh;                                           //创建节点句柄

    ros::Subscriber image_sub = nh.subscribe<sensor_msgs::Image>("/camera/aligned_depth_to_color/image_raw",1,depthCallback);   //订阅深度图像
    ros::Publisher target_pub = nh.advertise<std_msgs::Float32MultiArray>("/grasp_target", 10);

    ros::Rate rate(1.0);    //设定自循环的频率

    while(ros::ok)
    {

        tgt.data.clear();
        tgt.data.push_back((-tgtx + x_offset)/1000.0); //坐标转换 相机坐标系
        tgt.data.push_back((-tgty + y_offset)/1000.0);
        tgt.data.push_back((-low_value + z_offset)/1000.0);
        target_pub.publish(tgt);

        ros::spinOnce();
        rate.sleep();
    }
    return 0 ;
}


